//SkyTank Arduino interface
//Author: Leo Szeto
//Abstract: Arduino <--> Netbook interface for the SkyTank
//robot, designed to drive the motors, LEDs, and servo system

//THE ALL IMPORTANT SEMI-COLON
; //Forces the arduino to include its system header immediately

#define debug 0 //Debug Mode

//Define what each pin does what to help make coding easier
#define LMotorForw 5
#define LMotorBack 10
#define RMotorForw 9
#define RMotorBack 6

int IR_FL, IR_FR, IR_BL, IR_BR;
int IR_FL_Base, IR_FR_Base, IR_BL_Base, IR_BR_Base; //The basis measurements

boolean ardu_ready;
boolean FL_Stop, FR_Stop, BL_Stop, BR_Stop;
//Include any custom libraries we will add
#include "Skytank_lib.h"

Command cmd; //Create a command object

void setup()
{
  Serial.begin(9600);
  if(debug) Serial.println("SkyTanX Arduino Platform");
  if(debug) Serial.println("------------------------");
  cmd.instruction[INSTR_LEN] = '\0';
  cmd.parameter[INSTR_LEN] = '\0';
  //Setup a basis for the height of the sensor to ground
  //Maybe once IMU is setup, we should wait for car to settle first
  IR_FL_Base = analogRead(2);
  IR_FR_Base = analogRead(3);
  IR_BL_Base = analogRead(0);
  IR_BR_Base = analogRead(1);
  ardu_ready = false;
}
void loop()
{
 //Don't read until there are 8 bytes available
 //Each command consists of 8 bytes: This should
 //be more then enough to describe any cmd in human
 //readable form
 
 //Listen to see if anything useful has popped up on serial. 
 if(listen_serial(cmd))
 {
   //We have a command: parse it and update the machine state
   update(cmd);
 }
 //Now we execute any outstanding jobs
 if(ardu_ready)
 {
   //if(debug) Serial.println("Executing outstanding jobs.");
   execute();
 }
}
